#include "toroidal.h"
#include "math.h"
#include "cudafunc.h"

extern field Apsi;
particle::particle(double r,double z, double phi, double mu, double vpara)
{
    this->crd = {r, phi, z};
    this->mu = mu;
    this->vpara = vpara;
}

particle::particle(cylv ccrd, double mu, double vpara)
{
    this->crd = ccrd;
    this->mu = mu;
    this->vpara = vpara;
}

double particle::Pphi() //canonical angular momentum
{
    double Pphi = vpara*crd.r*m*rhoh_vs_a;
    Pphi += -q*Apsi.interp(crd);
    return Pphi;
}

double particle::energy(double B)
{
    return this->mu*B + m*vpara*vpara*0.5;
}

void particle::push(vec E,vec B, vec gB,vec curb,double dt)
{
    E = rotate(E,-this->crd.p);
    B = rotate(B,-this->crd.p);
    gB = rotate(gB,-this->crd.p);
    curb = rotate(curb,-this->crd.p);
    
    vec Bst = B + q*vpara/m*curb;
    double invBstn = sqrt(dot(B,B))/dot(Bst, B);    
    vec dX = vpara*B;
    vec curvdrift = q*vpara*vpara/m*curb;
    vec graddrift = -pow(dot(B,B),-0.5)*cross(B,E + -1*mu/q*gB);
    dX = dX + curvdrift + graddrift;
    dX = invBstn * dX;
    double dv = dot(Bst,E+(-1*mu/q)*gB)*q*invBstn/m;
    cylv x0c = this->crd;
    vec X0 = cyl2xyz(x0c);
    vec X = dt*dX + X0;
    /* diagnosis code */
    //print(curb);
    //print(curvdrift);
    //print(graddrift);
    // printf("R:%lf, phi:%lf, Z:%lf\n",x0c.r,x0c.p,x0c.z);
    // printf("Bsca:%lf\n",sqrt(dot(B,B)));
    // printf("delta:%lf\n", dot(gB,dX)*dt);
    // print(dt*dX);
    // print(gB);
    // printf("dEpara:%lf   ",vpara*dv*dt);
    // printf("dEperp:%lf\n", mu*dot(gB,dX)*dt);
    // printf("Epara:%lf   ",vpara*vpara*0.5);
    // printf("Eperp:%lf\n", mu*sqrt(dot(B,B)));
    this->crd = xyz2cyl(X);
    this->vpara = this->vpara + dt*dv;   
}

